#include "core.h"

/**
 * @brief  机体到地球
 * @param  void
 * @retval void
 */
void imuTransformVectorBodyToEarth(Axis3f * v, float angle)
{
  /* From body frame to earth frame */
	float x = v->x * cos(angle) + v->y * sin(angle);
	float y = v->y * cos(angle) - v->x * sin(angle);	
	
	v->x = x;
	v->y = y;
}


#define FILTER_NUM	10
#define FILTER_A	100.0f

/*限幅平均滤波法*/
void pressureFilter(float* in, float* out)
{	
	static uint8_t i=0, j=0, k;
	static float filter_buf[FILTER_NUM]={0.0};
	double filter_sum=0.0;
	uint8_t cnt=0;	
	float deta;		
	
	if(filter_buf[i] == 0.0f)	
	{
		/* 填满filter_buf */
		filter_buf[i] = *in;
		*out = *in;
		if(++i >= FILTER_NUM)	i=0;
	} else {
		
		/* 令deta等于第前5次的值 */
		if(i) {
			deta = *in-filter_buf[i-1];
		} else { 
			deta = *in-filter_buf[FILTER_NUM-1];
		}
		
		/* 更新filter_buf */
		if(fabs(deta) < FILTER_A)
		{
			filter_buf[i] = *in;
			if(++i >= FILTER_NUM)	{
				i=0;
			}
			j=0;
		} else if(++j > 100) {
			for (k=0; k<FILTER_NUM; k++)
			{
				filter_buf[k] = 0.0f;
			}
		}
		
		/* 计算累加和 */
		for(cnt=0; cnt<FILTER_NUM; cnt++)
		{
			filter_sum += filter_buf[cnt];
		}
		
		/* 计算平均 */
		*out = filter_sum / FILTER_NUM;
	}
}

/******************* (C) COPYRIGHT 2023 FUYEBING*****END OF FILE************/
